﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using MathLib;

namespace TrackingSim.Filtering
{
    class Kalman
    {
        public Kalman()
        {
        }

        public static void update_track_r6(ref Vector state, ref Matrix covariance, Matrix innovation, Matrix H, Matrix R)
        {
            Matrix S = (H * (covariance * H.tp())) + R;
            Matrix K = covariance * H.tp() * S.inverse();

            Matrix state_matrix = state.get_matrix_as_col();
            state_matrix = state_matrix + (K * innovation);

            state = state_matrix.get_col_vector(0);
            covariance = covariance - (K * H * covariance);
        }

        public static Matrix phi_pos_vel_accel_3state(double dt)
        {
            Matrix phi = new Matrix(3, 3);

            phi.m[0, 0] = 1.0; phi.m[0, 1] =  dt; phi.m[0, 2] = 0.5 * dt * dt;
            phi.m[0, 0] = 0.0; phi.m[0, 1] = 1.0; phi.m[0, 2] = dt; 
            phi.m[0, 0] = 0.0; phi.m[0, 1] = 0.0; phi.m[0, 2] = 1.0;

            return phi;
        }

        public static Matrix phi_pos_vel_6state(double dt)
        {
            Matrix phi = new Matrix(6, 6);

            phi.m[0, 0] = 1.0; phi.m[0, 1] = 0.0; phi.m[0, 2] = 0.0; phi.m[0, 3] =  dt; phi.m[0, 4] = 0.0; phi.m[0, 5] = 0.0;
            phi.m[1, 0] = 0.0; phi.m[1, 1] = 1.0; phi.m[1, 2] = 0.0; phi.m[1, 3] = 0.0; phi.m[1, 4] =  dt; phi.m[1, 5] = 0.0;
            phi.m[2, 0] = 0.0; phi.m[2, 1] = 0.0; phi.m[2, 2] = 1.0; phi.m[2, 3] = 0.0; phi.m[2, 4] = 0.0; phi.m[2, 5] =  dt;
            phi.m[3, 0] = 0.0; phi.m[3, 1] = 0.0; phi.m[3, 2] = 0.0; phi.m[3, 3] = 1.0; phi.m[3, 4] = 0.0; phi.m[3, 5] = 0.0;
            phi.m[4, 0] = 0.0; phi.m[4, 1] = 0.0; phi.m[4, 2] = 0.0; phi.m[4, 3] = 0.0; phi.m[4, 4] = 1.0; phi.m[4, 5] = 0.0;
            phi.m[5, 0] = 0.0; phi.m[5, 1] = 0.0; phi.m[5, 2] = 0.0; phi.m[5, 3] = 0.0; phi.m[5, 4] = 0.0; phi.m[5, 5] = 1.0;

            return phi;
        }
    }
}
